2.1 四旋翼物理模型

0. 前言

​ 如果你已经组装好了一架四旋翼，并且能用 PID 让它飞起来，下一步自然会问：能不能飞得更好？能不能让飞机自动跟踪轨迹、在大风中稳如磐石、甚至自己翻跟头？这些"进阶玩法"都需要一个共同的基石：数学模型。

​ 所谓数学模型，就是用微分方程精确描述"给四个电机多大的推力，飞机会怎么动"。有了它，LQR 能算出最优的反馈增益，MPC 能预判未来几步的状态变化，NMPC 能处理大角度翻转这样的极限机动。没有模型，这些算法都无从谈起：就像没有地图就无法规划路线一样。

​ 本文将从零开始，逐步建立四旋翼的完整非线性动力学模型：从符号定义到四元数运动学，从欧拉方程到全状态耦合。这是整个系列教程的理论地基，后续每一章都将在此之上构建。

读完本文后，你将能够：

• 明确四旋翼建模所需的全部物理参数（质量、转动惯量、力臂等）及其物理含义
• 使用四元数描述姿态运动学，理解其相比欧拉角的全局无奇异性优势
• 推导刚体转动的欧拉方程，理解陀螺耦合项的物理来源
• 写出四旋翼完整的 13 维非线性状态空间模型 $\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$
• 为后续线性化（2.2）、LQR（4.1）、MPC（5.1）、NMPC（6.1）等章节打下模型基础

---

目录

1. 符号定义与参数表
2. 纯姿态非线性模型
3. 全状态非线性模型
4. 模型总结

---

1. 符号定义与参数表

1.1 物理参数

符号	含义	单位
$m$	四旋翼总质量/起飞质量	kg
$g$	重力加速度	m/s²
$d$	电机力臂长度（电机到机体X/Y轴线的距离）	m
$k$	电机反扭矩/推力比例系数	—
$I_{xx},I_{yy},I_{zz}$	三轴转动惯量（假设主轴对齐，惯性张量为对角阵）	kg·m²

1.2 变量定义

符号	含义	单位
$\mathbf{q}=[q_0,q_1,q_2,q_3{]}^T$	姿态四元数（机体坐标系 → 世界坐标系）	—
$\mathit{\omega }=[{\omega }_x,{\omega }_y,{\omega }_z{]}^T$	机体坐标系三轴角速度	rad/s
$\mathbf{p}=[p_x,p_y,p_z{]}^T$	世界坐标系下的位置	m
$\mathbf{v}=[v_x,v_y,v_z{]}^T$	世界坐标系下的线速度	m/s
$\mathit{\tau }=[{\tau }_x,{\tau }_y,{\tau }_z{]}^T$	机体坐标系下的合外力矩	N·m
$t_i(i=1,2,3,4)$	第 $i$ 个电机的推力（控制输入）	N
$F_{\text{total}}$	四个电机总推力，$F_{\text{total}}=t_1+t_2+t_3+t_4$	N

2. 纯姿态非线性模型

纯姿态模型仅关注姿态动力学，该模型适用于姿态控制器（如PX4中的自稳模式控制器）的设计与分析。

2.1 状态与控制定义

状态向量（7 维）：

$${\mathbf{x}}_{\text{att}}=[q_0,q_1,q_2,q_3,{\omega }_x,{\omega }_y,{\omega }_z{]}^T$$

控制向量（4 维）：

$$\mathbf{u}=[t_1,t_2,t_3,t_4{]}^T$$

2.2 四元数运动学

四元数对时间的导数与角速度的关系由四元数乘法给出：

$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes {\mathit{\omega }}_q$$

其中 $\otimes$ 表示四元数乘法，${\mathit{\omega }}_q=[0,{\omega }_x,{\omega }_y,{\omega }_z{]}^T$ 为角速度的四元数表示（标量部分为 0）。

上式可改写为矩阵形式：

$$\dot{\mathbf{q}}=\frac{1}{2}\mathrm{\Omega }(\mathit{\omega })\mathbf{q}$$

其中：

$$\mathrm{\Omega }(\mathit{\omega })=\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]$$

展开后得到各分量的显式导数：

$$\begin{array}{rl}{\dot{q}}_0& =-\frac{1}{2}(q_1{\omega }_x+q_2{\omega }_y+q_3{\omega }_z)\\ {\dot{q}}_1& =\frac{1}{2}(q_0{\omega }_x-q_3{\omega }_y+q_2{\omega }_z)\\ {\dot{q}}_2& =\frac{1}{2}(q_3{\omega }_x+q_0{\omega }_y-q_1{\omega }_z)\\ {\dot{q}}_3& =\frac{1}{2}(q_1{\omega }_y-q_2{\omega }_x+q_0{\omega }_z)\end{array}$$

补充说明：

• 四元数导数描述的是四元数随角速度的变化率
• 因子 $1/2$ 源于四元数参数化对 $SO(3)$ 的双倍覆盖性质
• 每次数值积分后需对四元数归一化，以保持其单位长度约束

2.3 角速度动力学（欧拉方程）

螺旋桨推力在机体坐标系中产生的力矩为（Mujoco仿真坐标系）：

$$\begin{array}{rl}{\tau }_x& =d(t_1-t_2-t_3+t_4)\\ {\tau }_y& =d(-t_1-t_2+t_3+t_4)\\ {\tau }_z& =k(-t_1+t_2-t_3+t_4)\end{array}$$

注意： 力矩表达式中 $t_i$ 的符号取决于电机编号和机体坐标轴的定义方式，不同框架约定下符号可能不同。

对于刚体在机体坐标系中的旋转运动，欧拉方程为：

$$\mathit{\tau }=\mathbf{I}\dot{\mathit{\omega }}+\mathit{\omega }\times (\mathbf{I}\mathit{\omega })$$

其中 $\mathbf{I}=\text{diag}(I_{xx},I_{yy},I_{zz})$ 为惯性张量，表示机体分别绕三个轴的转动惯量。

展开得到：

$$\left[\begin{array}{c}{\tau }_x\\ {\tau }_y\\ {\tau }_z\end{array}\right]=\left[\begin{array}{c}{I}_{xx}{\dot{\omega }}_x+(I_{zz}-I_{yy}){\omega }_y{\omega }_z\\ I_{yy}{\dot{\omega }}_y+(I_{xx}-I_{zz}){\omega }_x{\omega }_z\\ I_{zz}{\dot{\omega }}_z+(I_{yy}-I_{xx}){\omega }_x{\omega }_y\end{array}\right]$$

整理得到角加速度的显式表达式：

$$\begin{array}{rl}{\dot{\omega }}_x& =\frac{{\tau }_x+(I_{yy}-I_{zz}){\omega }_y{\omega }_z}{I_{xx}}\\ {\dot{\omega }}_y& =\frac{{\tau }_y+(I_{zz}-I_{xx}){\omega }_x{\omega }_z}{I_{yy}}\\ {\dot{\omega }}_z& =\frac{{\tau }_z+(I_{xx}-I_{yy}){\omega }_x{\omega }_y}{I_{zz}}\end{array}$$

补充说明：

• 欧拉方程描述了外力矩作用下刚体角速度的变化率
• 交叉项 $(I_{yy}-I_{zz}){\omega }_y{\omega }_z$ 等为陀螺耦合项，体现了不同轴旋转运动之间的相互影响
• 模型假设四旋翼为对称刚体，惯性张量为对角矩阵

---

3. 全状态非线性模型

在前述纯姿态模型的基础上，增加质心平动自由度（位置和速度），得到完整的 13 维非线性模型。

3.1 状态与控制定义

状态向量（13 维）：

$$\mathbf{x}=[q_0,q_1,q_2,q_3,{\omega }_x,{\omega }_y,{\omega }_z,p_x,p_y,p_z,v_x,v_y,v_z{]}^T$$

控制向量（4 维，与纯姿态模型一致）：

$$\mathbf{u}=[t_1,t_2,t_3,t_4{]}^T$$

3.2 位置运动学

位置对时间的导数即为速度：

$$\begin{array}{rl}{\dot{p}}_x& =v_x\\ {\dot{p}}_y& =v_y\\ {\dot{p}}_z& =v_z\end{array}$$

3.3 速度动力学

四旋翼的平动受总推力和重力的共同作用。

总推力：

$$F_{\text{total}}=t_1+t_2+t_3+t_4$$

在机体坐标系中，推力始终沿机体 z 轴方向，加速度大小为：

$$a_{\text{thrust}}=\frac{F_{\text{total}}}{m}$$

利用四元数旋转矩阵将机体坐标系的推力加速度转换到世界坐标系。四元数 $\mathbf{q}$ 对应的旋转矩阵 ${\mathbf{R}}_b^w$（机体 → 世界）为：

$${\mathbf{R}}_b^w=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2(q_1^2+q_2^2)\end{array}\right]$$

世界坐标系下的推力加速度由 ${\mathbf{R}}_b^w$ 的第三列乘以 $a_{\text{thrust}}$ 得到：

$$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]={\mathbf{R}}_b^w\left[\begin{array}{c}0\\ 0\\ a_{\text{thrust}}\end{array}\right]$$

展开得到各分量的显式表达式：

$$\begin{array}{rl}{a}_x& =2(q_1{q}_3+q_0{q}_2)\cdot \frac{F_{\text{total}}}{m}\\ a_y& =2(q_2{q}_3-q_0{q}_1)\cdot \frac{F_{\text{total}}}{m}\\ a_z& =[1-2(q_1^2+q_2^2)]\cdot \frac{F_{\text{total}}}{m}\end{array}$$

考虑重力加速度 $g$（这里规定重力加速度沿世界坐标系Z轴负方向），速度的导数为：

$$\begin{array}{rl}{\dot{v}}_x& =a_x\\ {\dot{v}}_y& =a_y\\ {\dot{v}}_z& =a_z-g\end{array}$$

代入后得到：

$$\begin{array}{rl}{\dot{v}}_x& =2(q_1{q}_3+q_0{q}_2){\displaystyle \frac{F_{\text{total}}}{m}}\\ {\dot{v}}_y& =2(q_2{q}_3-q_0{q}_1){\displaystyle \frac{F_{\text{total}}}{m}}\\ {\dot{v}}_z& =[1-2(q_1^2+q_2^2)]{\displaystyle \frac{F_{\text{total}}}{m}}-g\end{array}$$

补充说明：

• 推力加速度的转换依赖于四元数姿态，这是姿态对平动产生影响的核心机制
• 四旋翼通过倾斜机体（改变姿态）来产生水平方向的加速度分量，从而实现位置控制
• 重力项仅在垂直方向（世界系 z 轴）参与补偿

3.4 四元数运动学

与纯姿态模型中的四元数运动学完全一致（参见 §2.2）：

$$\begin{array}{rl}{\dot{q}}_0& =-\frac{1}{2}(q_1{\omega }_x+q_2{\omega }_y+q_3{\omega }_z)\\ {\dot{q}}_1& =\frac{1}{2}(q_0{\omega }_x-q_3{\omega }_y+q_2{\omega }_z)\\ {\dot{q}}_2& =\frac{1}{2}(q_3{\omega }_x+q_0{\omega }_y-q_1{\omega }_z)\\ {\dot{q}}_3& =\frac{1}{2}(q_1{\omega }_y-q_2{\omega }_x+q_0{\omega }_z)\end{array}$$

3.5 角速度动力学

与纯姿态模型中的姿态动力学完全一致（参见 §2.3）：

$$\begin{array}{rl}{\dot{\omega }}_x& =\frac{{\tau }_x+(I_{yy}-I_{zz}){\omega }_y{\omega }_z}{I_{xx}}\\ {\dot{\omega }}_y& =\frac{{\tau }_y+(I_{zz}-I_{xx}){\omega }_x{\omega }_z}{I_{yy}}\\ {\dot{\omega }}_z& =\frac{{\tau }_z+(I_{xx}-I_{yy}){\omega }_x{\omega }_y}{I_{zz}}\end{array}$$

其中力矩由推力计算：

$$\begin{array}{rl}{\tau }_x& =d(t_1-t_2-t_3+t_4)\\ {\tau }_y& =d(-t_1-t_2+t_3+t_4)\\ {\tau }_z& =k(-t_1+t_2-t_3+t_4)\end{array}$$

3.6 完整系统非线性模型

将以上各部分汇总，得到完整的连续时间非线性系统模型 $\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$：

$$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\\ 2(q_1{q}_3+q_0{q}_2){\displaystyle \frac{F_{\text{total}}}{m}}\\ 2(q_2{q}_3-q_0{q}_1){\displaystyle \frac{F_{\text{total}}}{m}}\\ [1-2(q_1^2+q_2^2)]{\displaystyle \frac{F_{\text{total}}}{m}}-g\\ -\frac{1}{2}(q_1{\omega }_x+q_2{\omega }_y+q_3{\omega }_z)\\ \frac{1}{2}(q_0{\omega }_x-q_3{\omega }_y+q_2{\omega }_z)\\ \frac{1}{2}(q_3{\omega }_x+q_0{\omega }_y-q_1{\omega }_z)\\ \frac{1}{2}(q_1{\omega }_y-q_2{\omega }_x+q_0{\omega }_z)\\ {\displaystyle \frac{{\tau }_x+(I_{yy}-I_{zz}){\omega }_y{\omega }_z}{I_{xx}}}\\ {\displaystyle \frac{{\tau }_y+(I_{zz}-I_{xx}){\omega }_x{\omega }_z}{I_{yy}}}\\ {\displaystyle \frac{{\tau }_z+(I_{xx}-I_{yy}){\omega }_x{\omega }_y}{I_{zz}}}\end{array}\right]$$

其中 $F_{\text{total}}=t_1+t_2+t_3+t_4$，${\tau }_x,{\tau }_y,{\tau }_z$ 由 §3.5 的力矩公式给出。

---

4. 模型总结

本文建立的四旋翼非线性模型具有以下特点：

• 全局有效性： 四元数参数化避免了欧拉角在大角度机动时的奇异性问题，模型适用于全姿态范围的仿真与控制
• 完整的耦合关系： 模型完整反映了姿态对平动的影响（通过旋转矩阵）以及陀螺效应引起的轴间耦合
• 仿射控制结构： 控制输入 $t_1\sim t_4$ 通过力矩和总推力两种途径影响系统状态

本章参考资料与引申阅读已汇总至 参考资料。